/*****************************************************************
**                ����������ѧ ��BUGս��
**                **������������������**
**    û��ʲô������һ��ͨ��������˵ģ�������Ǿ���������
**---------------------------------------------------------------
** ��Ŀ���ƣ�   mxdemo_std_robot
** ��    �ڣ�   2021-04-15
** ��    �ߣ�   MasterWang
**---------------------------------------------------------------
** �� �� ����   motor_control.c
** �ļ�˵����   �����˵�����������ݿ��ƺ��˶�����
*****************************************************************/

/*---------------------INCLUDES----------------------*/
#include "motor_control.h"
#include "referee_system.h"

#include "can.h"
#include "tim.h"
#include "usart.h"
#include "bmi088.h"
#include "IMU.h"
#include "PID.h"

/*---------------------VARIABLES---------------------*/
//ʹ��CCM����RAM��(0x10004000-0x10004FFF 4KB)
//
Motor_Status_t motor_status_chassis[4] __attribute__((at(0x10004000))) = {0};
Motor_Status_t motor_status_gimbal[2] __attribute__((at(0x10004200))) = {0};
Motor_Status_t motor_status_ammobooster[3] __attribute__((at(0x10004400))) = {0};
Motor_Status_Ex_t motor_status_ex_turnbullet __attribute__((at(0x10004600))) = {0};

float gimbal_angle_deg[2] = {0.0f};

//
Robot_Motor_Speed_t robot_motor_speed __attribute__((at(0x10004800))) = {0};

//
float supercap_voltage __attribute__((at(0x10004A00))) = 0.0f;
float supercap_energy_percent __attribute__((at(0x10004A10))) = 0.0f;

//
uint8_t chassis_reset_id_flag __attribute__((at(0x10004A20))) = 0; //������̵������ID���ó���ı�־λ

/*---------------------FUNCTIONS---------------------*/

/***********************************************************************
** �� �� ���� UpdateMotorStatus()
** ����˵���� ���������»����˵������״̬(CAN�����жϵ���)
**---------------------------------------------------------------------
** ��������� ���״̬����ָ��,CAN����ԭʼ����
** ���ز����� ��
***********************************************************************/
void UpdateMotorStatus(Motor_Status_t *ptr, uint8_t *data)
{
	//
	ptr->angle = (uint16_t)((data[0]<<8)|data[1]);
	ptr->speed_rpm = (int16_t)((data[2]<<8)|data[3]);
	ptr->current = (int16_t)((data[4]<<8)|data[5]);
	ptr->temperature = data[6];
}

/***********************************************************************
** �� �� ���� UpdateMotorStatusEx()
** ����˵���� ���»����˵����չ״̬(CAN�����жϵ���)
**---------------------------------------------------------------------
** ��������� �����չ״̬����ָ��,�������״̬����ָ��
** ���ز����� ��
***********************************************************************/
void UpdateMotorStatusEx(Motor_Status_Ex_t *ptr_ex, Motor_Status_t *ptr)
{
	//
	ptr_ex->delt_angle = ptr->angle - ptr_ex->last_angle;
	
	if(ptr_ex->delt_angle<-4096){ptr_ex->round_cnt++;}
	else if(ptr_ex->delt_angle>4096){ptr_ex->round_cnt--;}
	
	ptr_ex->total_angle = ptr_ex->round_cnt*8192 + ptr->angle - ptr_ex->offset_angle;
	
	ptr_ex->last_angle = ptr->angle;
}

/***********************************************************************
** �� �� ���� HAL_CAN_RxFifo0MsgPendingCallback()
** ����˵���� CAN_FIFO0�����жϻص�����
**---------------------------------------------------------------------
** ��������� CAN���
** ���ز����� ��
***********************************************************************/
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8];
	uint8_t i;

	HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
	
	if(hcan==&hcan1)
	{
		switch (rx_header.StdId)
		{
			case CAN_3508_MOTOR1_ID: 
			case CAN_3508_MOTOR2_ID:
			case CAN_3508_MOTOR3_ID:
			case CAN_3508_MOTOR4_ID:
			{
				i = rx_header.StdId - CAN_3508_MOTOR1_ID;
				UpdateMotorStatus(&motor_status_chassis[i], rx_data);
				break;
			}
			default: break;
		}
	}
	else if(hcan==&hcan2)
	{
		switch (rx_header.StdId)
		{
			case CAN_3508_SHOOT1_ID:
			{
				UpdateMotorStatus(&motor_status_ammobooster[0], rx_data);
				break;
			}
			case CAN_6020_PIT_ID:
			{
				UpdateMotorStatus(&motor_status_gimbal[1], rx_data);
				motor_status_gimbal[1].angle=(motor_status_gimbal[1].angle+4324)%8192; //����ƫ��6020���ԽǶ�
				gimbal_angle_deg[1] = -1*(motor_status_gimbal[1].angle-PIT6020_HORIZONTAL_POSITION)*360.0f/8192;
				break;
			}
			case CAN_3508_SHOOT2_ID:
			{
				UpdateMotorStatus(&motor_status_ammobooster[1], rx_data);
				break;
			}
			case CAN_2006_TURNBULLET_ID:
			{
				UpdateMotorStatus(&motor_status_ammobooster[2], rx_data);
				UpdateMotorStatusEx(&motor_status_ex_turnbullet, &motor_status_ammobooster[2]);
				break;
			}
			default: break;
		}
	}
}

/***********************************************************************
** �� �� ���� HAL_CAN_RxFifo1MsgPendingCallback()
** ����˵���� CAN_FIFO1�����жϻص�����
**---------------------------------------------------------------------
** ��������� CAN���
** ���ز����� ��
***********************************************************************/
void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan)
{
	CAN_RxHeaderTypeDef rx_header;
	uint8_t rx_data[8];
	uint8_t *ptr;

	HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO1, &rx_header, rx_data);
	
	if(hcan==&hcan1)
	{
		switch (rx_header.StdId)
		{
			case CAN_6020_YAW_ID:
			{
				UpdateMotorStatus(&motor_status_gimbal[0], rx_data);
				motor_status_gimbal[0].angle=(motor_status_gimbal[0].angle+3241)%8192; //����ƫ��6020���ԽǶ�
				gimbal_angle_deg[0] = (motor_status_gimbal[0].angle-YAW6020_45DEGREE_POSITION)*360.0f/8192;
				break;
			}
			case CAN_SUPERCAP_STAT1_ID:
			{
				ptr=(uint8_t *)&supercap_voltage; ptr[0]=rx_data[0]; ptr[1]=rx_data[1]; ptr[2]=rx_data[2]; ptr[3]=rx_data[3];
				ptr=(uint8_t *)&supercap_energy_percent; ptr[0]=rx_data[4]; ptr[1]=rx_data[5]; ptr[2]=rx_data[6]; ptr[3]=rx_data[7];
				break;
			}
			default: break;
		}
	}
}

/***********************************************************************
** �� �� ���� CAN_cmd_chassis_reset_ID()
** ����˵���� ����3508��������������ID����(�̰�SET�ٳ���SET)
**---------------------------------------------------------------------
** ��������� ��
** ���ز����� ��
***********************************************************************/
void CAN_cmd_chassis_reset_ID(void)
{
	CAN_TxHeaderTypeDef tx_header;
	uint8_t tx_data[8];
	uint32_t tx_mail_box;
	
	tx_header.StdId = 0x700;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 0x08;
	
	tx_data[0] = 0;
	tx_data[1] = 0;
	tx_data[2] = 0;
	tx_data[3] = 0;
	tx_data[4] = 0;
	tx_data[5] = 0;
	tx_data[6] = 0;
	tx_data[7] = 0;
	
	HAL_CAN_AddTxMessage(&hcan1, &tx_header, tx_data, &tx_mail_box);
}

/***********************************************************************
** �� �� ���� CAN_cmd_motor()
** ����˵���� CAN���͵������ָ���������
**---------------------------------------------------------------------
** ��������� can���,id,����1,����2,����3,����4
** ���ز����� ��
***********************************************************************/
void CAN_cmd_motor(CAN_HandleTypeDef *hcan, uint32_t identifier, int16_t data1, int16_t data2, int16_t data3, int16_t data4)
{
	CAN_TxHeaderTypeDef tx_header;
	uint8_t tx_data[8];
	uint32_t tx_mail_box;
	
	tx_header.StdId = identifier;
	tx_header.IDE = CAN_ID_STD;
	tx_header.RTR = CAN_RTR_DATA;
	tx_header.DLC = 0x08;
	
	tx_data[0] = (data1 >> 8);
	tx_data[1] = data1;
	tx_data[2] = (data2 >> 8);
	tx_data[3] = data2;
	tx_data[4] = (data3 >> 8);
	tx_data[5] = data3;
	tx_data[6] = (data4 >> 8);
	tx_data[7] = data4;
	
	HAL_CAN_AddTxMessage(hcan, &tx_header, tx_data, &tx_mail_box);
}

/***********************************************************************
** �� �� ���� CAN_cmd_chassis()
** ����˵���� ���͵��̵������ָ��
**---------------------------------------------------------------------
** ��������� ���̵��1����ֵ,���̵��2����ֵ,���̵��3����ֵ,���̵��4����ֵ
** ���ز����� ��
***********************************************************************/
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
{
	//���̵��->can1
	CAN_cmd_motor(&hcan1, CAN_MOTOR_CTRL_ID_1_4, motor1, motor2, motor3, motor4);
}

/***********************************************************************
** �� �� ���� CAN_cmd_gimbal_ammobooster()
** ����˵���� ������̨�ͷ�������������ָ��
**---------------------------------------------------------------------
** ��������� ��̨yaw�������ֵ,��̨pitch�������ֵ,Ħ���ֵ��1����ֵ,Ħ���ֵ��2����ֵ,�����������ֵ
** ���ز����� ��
***********************************************************************/
void CAN_cmd_gimbal_ammobooster(int16_t yaw, int16_t pitch, int16_t shoot1, int16_t shoot2, int16_t turnbullet)
{
	//yaw���->can1, pitch���->can2
	//Ħ���ֵ��->can2, �������->can2
	CAN_cmd_motor(&hcan1, CAN_MOTOR_CTRL_ID_5_8, yaw, 0, 0, 0);
	CAN_cmd_motor(&hcan2, CAN_MOTOR_CTRL_ID_5_8, shoot1, pitch, turnbullet, shoot2);
}

/***********************************************************************
** �� �� ���� set_chassis_motor_speed()
** ����˵���� ���Ƶ��̵��ת��(��500Hz��ʱ���жϵ���)
**---------------------------------------------------------------------
** ��������� �����ĸ����ӵ���ٶ��趨ֵ
** ���ز����� ��
***********************************************************************/
void set_chassis_motor_speed(float wheel_speed[])
{
	uint8_t i;
	int16_t output_chassis[4];
	
	for(i=0;i<4;i++)
	{
		pid_chassis[i].Ref = wheel_speed[i];
		output_chassis[i] = PIDIncControl(&pid_chassis[i], motor_status_chassis[i].speed_rpm);
	}
	
	//
	CAN_cmd_chassis(output_chassis[0], output_chassis[1], output_chassis[2], output_chassis[3]);
}

/***********************************************************************
** �� �� ���� set_gimbal_ammobooster_motor_speed()
** ����˵���� ������̨������������ת��(��500Hz��ʱ���жϵ���)
**---------------------------------------------------------------------
** ��������� ��̨yaw���趨�ٶ�,��̨pitch���趨�ٶ�,Ħ�����趨�ٶ�,��������趨ת�٣���̨����ʹ��bmi088�����ǵĽ��ٶ���Ϊ����ֵ��
** ���ز����� ��
***********************************************************************/
void set_gimbal_ammobooster_motor_speed(int16_t yaw_speed, int16_t pitch_speed, int16_t shoot_speed, int16_t turnbullet_speed)
{
	float yaw_axis_speed, pitch_axis_speed;
	int16_t output_yaw, output_pitch, output_shoot[2], output_turnbullet;
	
	//
	pid_gimbal[0].Ref = yaw_speed;
	
	arm_sqrt_f32(bmi088_gyro_deg_calib[BMI088_GYRO_YAW_INDEX]*bmi088_gyro_deg_calib[BMI088_GYRO_YAW_INDEX]+bmi088_gyro_deg_calib[BMI088_GYRO_ROLL_INDEX]*bmi088_gyro_deg_calib[BMI088_GYRO_ROLL_INDEX],&yaw_axis_speed);
	if(bmi088_gyro_deg_calib[BMI088_GYRO_YAW_INDEX]<0){yaw_axis_speed=-yaw_axis_speed;}
	output_yaw = PIDIncControl(&pid_gimbal[0], yaw_axis_speed);  //������,�޸�����������
	
	//
	pid_gimbal[1].Ref = pitch_speed;
	pitch_axis_speed = bmi088_gyro_deg_calib[BMI088_GYRO_PITCH_INDEX];
	output_pitch = -1*PIDIncControl(&pid_gimbal[1], pitch_axis_speed); //������,�޸�����������
	
	//3508Ħ����
	pid_ammobooster[0].Ref = -shoot_speed;  //������,�޸�����������
	output_shoot[0] = PIDIncControl(&pid_ammobooster[0], motor_status_ammobooster[0].speed_rpm);
	
	pid_ammobooster[1].Ref = shoot_speed; //������,�޸�����������
	output_shoot[1] = PIDIncControl(&pid_ammobooster[1], motor_status_ammobooster[1].speed_rpm);
	
	//snailĦ����
	//__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, shoot_speed);
	//__HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_2, shoot_speed);

	//
	pid_ammobooster[2].Ref = -turnbullet_speed; //������,�޸�����������
	output_turnbullet = PIDIncControl(&pid_ammobooster[2], motor_status_ammobooster[2].speed_rpm);

	//
	CAN_cmd_gimbal_ammobooster(output_yaw, output_pitch, output_shoot[0], output_shoot[1], output_turnbullet); //���������ٶ�ʱ�޸�����,������Ƶĵ����0
}

/***********************************************************************
** �� �� ���� set_gimbal_imu_angle()
** ����˵���� ʹ��IMU�����Ƕȿ�����̨�Ƕ�λ��(�ڻ������˶������е���)
**---------------------------------------------------------------------
** ��������� ��̨yaw���趨�Ƕ�,��̨pitch���趨�Ƕ�
** ���ز����� ��
***********************************************************************/
void set_gimbal_imu_angle(float yaw_angle, float pitch_angle)
{
	//
	pid_yaw.Ref = yaw_angle;
	robot_motor_speed.yaw_speed = PIDPosControl(&pid_yaw, IMU_yaw_angle_dial);
	
	//
	pid_pitch.Ref = pitch_angle;
	robot_motor_speed.pitch_speed = PIDPosControl(&pid_pitch, IMU_angle_deg[IMU_PITCH_INDEX]);
}

/***********************************************************************
** �� �� ���� set_gimbal_6020_angle()
** ����˵���� ʹ��6020�����Ƕȿ�����̨�Ƕ�λ�ã���ר�����������ػ�����Ҫ���̾�ֹ��45��Ϊyaw��㡢���޶�yaw��Χ��45��(�ڻ������˶������е���)
**---------------------------------------------------------------------
** ��������� ��̨yaw���趨�Ƕ�,��̨pitch���趨�Ƕ�
** ���ز����� ��
***********************************************************************/
void set_gimbal_6020_angle(float yaw_angle, float pitch_angle)
{
	//
	pid_yaw.Ref = yaw_angle;
	robot_motor_speed.yaw_speed = PIDPosControl(&pid_yaw, gimbal_angle_deg[0]);
	
	//
	pid_pitch.Ref = pitch_angle;
	//robot_motor_speed.pitch_speed = PIDPosControl(&pid_pitch, gimbal_angle_deg[1]);
	robot_motor_speed.pitch_speed = PIDPosControl(&pid_pitch, IMU_angle_deg[IMU_PITCH_INDEX]);
}

/***********************************************************************
** �� �� ���� robot_motion_resolving()
** ����˵���� �������˶�����,����ָ��->����ٶȽ���(��200Hz��ʱ���жϵ���)
**---------------------------------------------------------------------
** ��������� �����˿���ָ�������ַ, �����˵���ٶȱ�����ַ
** ���ز����� ��
***********************************************************************/
void robot_motion_resolving(ROBOT_ctrl_t *ctrl, Robot_Motor_Speed_t *motor_speed)
{
	uint8_t i;
	
	float tmp;
	float max_wheelspeed=0.0f;
	float wheelspeed_scale=1.0f;
	static float chassis_motor_maxspeed=5000.0f;

	float vw_set=0.0f; //rpm
	float vx_resolve,vy_resolve,vw_resolve; //mm/s
	
	/******************�����˶��ֽ�***************/	
	//vw_set����
	if(ctrl->movement_mode==CHASSIS_FOLLOW_GIMBAL) //���̸�����̨
	{
		if(ctrl->chassis_lock_flag==1) //������ת����
		{
			vw_set = 0.0f;
		}
		else
		{
			pid_rotation.Ref = YAW6020_FORWARD_POSITION*360.0f/8192; //����Ե�
			
			vw_set = -1*PIDPosControl(&pid_rotation, motor_status_gimbal[0].angle*360.0f/8192); //������,�޸�����������
		}
	}
	else if(ctrl->movement_mode==CHASSIS_GYRO_ROTATION) //����С������ת
	{
		if(ctrl->chassis_accelerate_flag==1){vw_set = 60.0f;}
		else{vw_set = 45.0f;}
	}
	
	//�����ĸ����ֵ���ٶȽ��� mm/s->rpm
	float32_t angle = (YAW6020_FORWARD_POSITION-motor_status_gimbal[0].angle)*6.2831853f/8192; //�򵥲���ʱangle=0
	
	vx_resolve = ctrl->vx_set*arm_cos_f32(angle) + ctrl->vy_set*arm_sin_f32(angle);
	vy_resolve = ctrl->vy_set*arm_cos_f32(angle) - ctrl->vx_set*arm_sin_f32(angle);
	vw_resolve = vw_set*41.78371f; //sqrt(2)*v*cos(45-arctan(378/418))=(w/60)*2*PI*sqrt(378*378+418*418)
	
	motor_speed->chassis_speed[0] = (-(vx_resolve + vy_resolve + vw_resolve)) * 2.40494601831f; // v_set/(pi*152.5) * 60 * (3591/187)
	motor_speed->chassis_speed[1] = (vx_resolve - vy_resolve - vw_resolve) * 2.40494601831f;
	motor_speed->chassis_speed[2] = (vx_resolve + vy_resolve - vw_resolve) * 2.40494601831f;
	motor_speed->chassis_speed[3] = (-(vx_resolve - vy_resolve + vw_resolve)) * 2.40494601831f;
	
	//�������ݵ�ѹ����
	if(supercap_energy_percent <= 20.0f) {chassis_motor_maxspeed = chassis_motor_maxspeed*0.9f+(4000+(supercap_energy_percent-20.0f)*200)*0.1f;}
	else {chassis_motor_maxspeed = 4000.0f;}

	//С���ݺͼ����ƶ�ʱ,���ӵ��ݵ�ѹ����
	if((ctrl->movement_mode==CHASSIS_GYRO_ROTATION)||(ctrl->chassis_accelerate_flag==1)) {chassis_motor_maxspeed = 8000.0f;}
	
	//�������ӵ�����ٶ�
	for(i=0;i<4;i++)
	{
		tmp = ABS(motor_speed->chassis_speed[i]);
		if(tmp > max_wheelspeed) {max_wheelspeed = tmp;}
	}
//	if(max_wheelspeed > chassis_motor_maxspeed)
//	{
//		wheelspeed_scale = chassis_motor_maxspeed / max_wheelspeed;
//		for(i=0;i<4;i++) {motor_speed->chassis_speed[i] *= wheelspeed_scale;}
//	}
	
	/******************��̨�Ƕȿ���*************************/
	//
	if(ctrl->energy_aiming_flag==1) //��������ģʽ
	{
		set_gimbal_6020_angle(ctrl->yaw_angle_set_energy, ctrl->pitch_angle_set);
	}
	else //��ͨģʽ
	{
		set_gimbal_imu_angle(ctrl->yaw_angle_set, ctrl->pitch_angle_set);
	}
	
	//
	uint8_t tx_buf[5]={0};
	int16_t tmp_pitch = (IMU_angle_deg[IMU_PITCH_INDEX])*100; 
	tx_buf[0]=0x55; tx_buf[1]=0x11; tx_buf[2]=(tmp_pitch>>8); tx_buf[3]=tmp_pitch; tx_buf[4]=(tx_buf[1]+tx_buf[2]+tx_buf[3]);
	SendUART6DataPackage(tx_buf, 5);
	
	
	/******************Ħ���ֺͺ�㼤��������***************/
	//
	if(ctrl->shoot_open_flag==1)
	{
		HAL_GPIO_WritePin(LASER_GPIO_Port,LASER_Pin,GPIO_PIN_SET); //open laser
		
		//3508Ħ���� 0-8000
		if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==15) {motor_speed->shoot_speed = 7000;}
		else if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==18) {motor_speed->shoot_speed = 7000;}
		else if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==30) {motor_speed->shoot_speed = 7000;}
		else {motor_speed->shoot_speed = 7000;}//����Ħ����ת�٣���ֵԽ��ת��Խ��
		
		//snailĦ���� 400-2200
		//if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==15) {motor_speed->shoot_speed = 1260;}
		//else if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==18) {motor_speed->shoot_speed = 1300;}
		//else if(robot_referee_status.game_robot_status.shooter_id1_17mm_speed_limit==30) {motor_speed->shoot_speed = 1600;}
		//else {motor_speed->shoot_speed = 1260;}
	}
	else
	{
		HAL_GPIO_WritePin(LASER_GPIO_Port,LASER_Pin,GPIO_PIN_RESET); //close laser
		
		motor_speed->shoot_speed = 0; //3508Ħ����
		//motor_speed->shoot_speed = 400; //snailĦ����
	}
	
	/******************������������***************/
	//
	if((ctrl->turnbullet_flag==1)||(ctrl->turnbullet_once_flag==1))
	{
		//�ж�ǹ����������(��סZ����)
		if((robot_referee_status.power_heat_data.shooter_id1_17mm_cooling_heat>=(robot_referee_status.game_robot_status.shooter_id1_17mm_cooling_limit*0.7))&&(ctrl->ignore_shoot_heat_limit_flag==0))
		{
			motor_speed->turnbullet_speed = 0;
		}
		else
		{
			uint16_t turnbullet_freq;
			
			//������ȴ���ʵ�����Ƶ ����40 60 80�����������Ƶ
			if(ctrl->turnbullet_once_flag==1)
			{
				turnbullet_freq = 5;
			}
			else
			{
				if(ctrl->auto_aiming_flag==1)
				{
					turnbullet_freq = 3*(robot_referee_status.game_robot_status.shooter_id1_17mm_cooling_rate/10);
					if(turnbullet_freq>30){turnbullet_freq=30;} //�����Ƶ20Hz
				}
				else
				{
					turnbullet_freq = 2*(robot_referee_status.game_robot_status.shooter_id1_17mm_cooling_rate/10);
					if(turnbullet_freq>25 ){turnbullet_freq=25;} //�����Ƶ16Hz
				}
			}
			
			motor_speed->turnbullet_speed =  turnbullet_freq * 308.57143f; //Hz->rpm (shoot_freq*60/7)*36
		}
	}
	else if(ctrl->turnbullet_reverse_flag==1)
	{
		motor_speed->turnbullet_speed = -600.0f; //~2Hz
	}
	else
	{
		motor_speed->turnbullet_speed = 0;
	}
	
	
	/******************���ոǶ�������***************/
	//
	if(ctrl->bulletroom_open_flag==1)
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,BULLETROOM_COVER_OPEN); //open
	}
	else
	{
		__HAL_TIM_SET_COMPARE(&htim8,TIM_CHANNEL_1,BULLETROOM_COVER_CLOSE); //close
	}
}

/***********************************************************************
** �� �� ���� robot_motor_speed_control_test()
** ����˵���� �������ٶȻ����Ƶ���
**---------------------------------------------------------------------
** ��������� ��
** ���ز����� ��
***********************************************************************/
void robot_motor_speed_control_test(void)
{
	float vx,vy,vw;

	//
	vx = rc_ctrl.ch3*10.0f;
	vy = -1.0f*rc_ctrl.ch4*10.0f;
	vw = rc_ctrl.wheel*5.0f;
	
	robot_motor_speed.chassis_speed[0] = -(vx + vy + vw);
	robot_motor_speed.chassis_speed[1] = vx - vy - vw;
	robot_motor_speed.chassis_speed[2] = vx + vy - vw;
	robot_motor_speed.chassis_speed[3] = -(vx - vy + vw);
	
	//
	robot_motor_speed.yaw_speed = rc_ctrl.ch1*0.2f;
	robot_motor_speed.pitch_speed = rc_ctrl.ch2*0.2f;
	
	//
	HAL_GPIO_WritePin(LASER_GPIO_Port,LASER_Pin,GPIO_PIN_SET); //open laser
}
